{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Implementing Dubins Curves\n",
    "This notebook is intended for testing different libraries or formulations for Dubins curve type path sets to be used for simulation and field planning work in an adaptive sampling/informative path planning regime.\n",
    "\n",
    "Please note, that you may need to install the following in order to play with the [dubins library](https://github.com/AndrewWalker/Dubins-Curves):\n",
    "\n",
    "```pip install dubins```\n",
    "\n",
    "This library allows for the generation of dubins curves assuming that the turning radius is known. It also provides sample points along the curve given some sampling step.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Necessary Imports\n",
    "import dubins\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Learning the Dubins Library\n",
    "The dubins library used here is a cython wrapper for a more powerful C++ library. The methods relevant to our implementation are demonstrated and documented inline below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7fdcbec26350>]"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7fdcbecf5490>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Generating the shortest path; the library internally cycles through different dubins curve paths to select the\n",
    "# shortest length path.\n",
    "\n",
    "#starting location (x,y,yaw)\n",
    "q0 = (0, 0, 0)\n",
    "\n",
    "#goal location (x,y,yaw)\n",
    "q1 = (10, 10, 3.14)\n",
    "\n",
    "#radius of feasible turn\n",
    "turning_radius = 1.0\n",
    "\n",
    "#sampling \"rate\" or distance\n",
    "step_size = 0.5\n",
    "\n",
    "#find the shortest path, returns a dubins object\n",
    "path = dubins.shortest_path(q0, q1, turning_radius)\n",
    "\n",
    "#sample the path and return the points along the path\n",
    "#methods to apply on the path include sample_many and sample\n",
    "configurations, _ = path.sample_many(step_size)\n",
    "\n",
    "#plot the path\n",
    "x = []\n",
    "y = []\n",
    "for sample_points in configurations:\n",
    "    x.append(sample_points[0])\n",
    "    y.append(sample_points[1])\n",
    "plt.plot(x,y,'r*')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAlsAAADFCAYAAABjNjdrAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzt3Xt8FPW9//HXN9fdWBdRqKxaC1ovBTxgjVbxEuuV6rGK\nKHd/9rQaArW0teipirdNrW3VemoPrOINWlHRNsX7EbxitIqhVkQR0QUV2AQMFhCyIcl+f3/MICEm\nkMvObnbyfj4eecAuy85355OZ/cx3PvMZY61FRERERLyRk+kBiIiIiPiZki0RERERDynZEhEREfGQ\nki0RERERDynZEhEREfGQki0RERERDynZEhEREfGQki0RERERDynZEhEREfFQXqYH0FyfPn1s//79\nMz2MHmvx4sWfWWv7pur9FM/MSmU8FcvM0rbpL9o2/aO9sexWyVb//v2pqqrK9DB6LGPMx6l8P8Uz\ns1IZT8Uys7Rt+ou2Tf9obyx1GlFERETEQ0q2RERERDykZEtERETEQ0q2RERERDykZEtERETEQ0q2\nRERERDykZEtERETEQ0q2RERERDykZEtERETEQ0q2RERERDykZEtERETEQ0q2RERERDykZEtERETE\nQ0q2RERERDykZEtERETEQ0q2RERERDyUkmTLGHOfMWadMWZps+duMMasMcb8y/05KxXLEhEREckm\nqZrZmgUMb+X52621Q92fp1O0LBEREZGskZJky1q7ENiQivcSERER8ROva7YuM8YscU8z9m7tBcaY\nUmNMlTGmav369R4PR7ymePqHYukviqd/KJbZx8tkKwocDAwF4sBtrb3IWjvTWltsrS3u27evh8OR\ndFA8/UOx9BfF0z8Uy+zjWbJlra2x1jZZa5PA3cAxXi1LREREpLvyLNkyxoSbPRwBLG3rtSIiIiJ+\nlZeKNzHGPAScDPQxxqwGrgdONsYMBSywCpiYimWJiIiIZJOUJFvW2rGtPH1vKt5bREREJJupg7yI\niIiIh5RsiYiIiHhIyZaIiIiIh5RsiYiIiHhIyZaIiIiIh5RsiYiIiHhIyZaIiIiIh5RsiYikQzwO\nJSVQXZ3pkfQcWufSTSjZ8gPtUDJH617aq7wcKishEsn0SHoOrXPpJpRs+YCNlGNfqSR5Qxs7FCUE\nnrAWNlxejtXOXHYlGARjIBqFZNL50xjnefGG1rl0M0q2spm7QzF3RjE2Sc5dzg6lIT/IggWwebP7\nOh3dpZS10FQYxOQY9n44imm2M08Gmu3MleT2LC3inUjAU0/BFSNj/C0wji0UAbAtr4i688fDypWZ\nHK0/tLKNLVsGN14c46+FO6/zf5/dYp1r+5Q0UrKVpdatgysuiDGHcWxttkN5aq/xfLNpJWecAXkh\nHd11WbMdsrUwfz6ccAIcsC1GRXAcDfnOuq8zRTzAeParX8nAgXDllbB2kma9epRyJ97vj48wciT0\n6QP/+Z9w52NhvrZfiCKTYFtOgNzGBH9+LMTPbu7H6tXu/9UXf+e4B5LbpkW4914YNgwGDoRf3xvm\na+Gd1/mDT4U44YJ+/OUvUFeHDkIlvay13ebnqKOOsrILa9faphNOsjOui9tQyNq8PGsrjyizyZwc\nawMBa3NyrJ00yW7caO2zz1r7+1+stQv2HWe3UGQt2MbCImvHj7c2Hm/17YEqq3jubNIkm8zJsavO\nmmS/+11rwdoDDrB2+nRrGy4pc9a5u+7/PW6S/Z//sTaRE3Be2PInEEjr0FMZT1/E0iuB1uO9LTdg\nn3nG2kTCWjtihLWTJ1v7r3/ZDWMn2zcPHGFzc60tKLB24kRrN46f9OX22xptmy20sc7rTMDecou1\n1dV2p3W+9b8m2+WDR9hDDrF2K5nfPrVt+kd7Y5nR5Krlj35p2pZMWrvy+5NsEzl2OpPs979v7bJl\ndqcdip082XncXJmTjNWZgG0kxyZ+1PrO3Frt0HfSxs68IS/gfHla2/a6X7vW1l8wzjYUOEluInfX\nSa5XtENPk7Vr7cZzxtkvmh3UJMftPt6xmLXbctv3xa9ts4W1a21i5I4DyURukV13xnibXLvrdZ5M\nWvvKI2vtKwfuiFcymP7tU9umf7Q3lnmZmlGTDggGMYkE/d2Hk4ky+ZkovBhw58Nd06d/9f/W1GDK\nyvhwWCkLJ8zku8/FOSodY852sRhMnUr93HkUNm2lsaAIc/4I8m6/lbxC9zUVFTte33zdh8MU9AlB\nY4KGvAB5jQniW0KE+/VL60eQNAmHeXVJiDNIYAsD5DYkoFcIdhPvAQOAT2NsnTyVnMfnEUhuxQad\n3zNuvTU9Y89Stl+YFxY76zxZEKCwMUHfg0MQ3vU6NwZOuDAML4SwdyWoswECiQSEdh8vka5QzVY2\niMVIjh1HY4FTH0RREYxvZ4FtRQVMn87g8UNYc/V0ij+p4MknvR2uL4TDbGgKkdeUoCHXSZhye3dg\nh1xTA2VlNL7yOn/Zo4xlL1bT1OTtkCUzFiyArR/X8PaxZZg3XoeysvbXXoXDFPULUWgT1BFwqur1\nxb9bd90FW1fV8M6wMnIWdXCdA9TUYCeWMfqbr/PoPmVY1cqJxzSzlQ3CYXJ6hchpTECg8zvk666D\nJ56A0lJ497k4vSeNgblztWNvwwev1PBOfhkjny1l77/OdIqY28ud9QoCgXumc+pYmHkfXHqpN2OV\nzGhshJ//HBIHVfDeS0Ahrc8w70pNDWZSGT98vpTzP5vJ6OoO/J71QEuXwi9+AScPr+Cpp3CmDDq6\nzisqyAHOvRdGXzKd3pPgdA/GKrKdZrayhTtTwuudOIpzFRbCrFmwfj0sPk9X4uzKokVw3NoK4tOm\ns/f3hjg78+anDTtg9Gg48UT446/iNByvK858Ix6n5vASat+r5rbbnO2rU9zZ55IpQxhTO51/Tuvc\n75nvxeM0nVjCZRdU06uXsy/L6eI32IQJsN9+cPPNKRmhSJtSkmwZY+4zxqwzxixt9tzexpgFxpgV\n7p+9U7GsHsvdITOka1/83zk+SEOj4bQVagfRFrs2TuEZJQzap5pf/KLr72cM3HEH/GRDObmvKcH1\ni8Q15fT7qJK79otw7rldf79x45yJ60fvUBuIVpWXYyorGbU8wuzZsO++XX/LwkL45S9h2YtxNh2p\ndS7eSdXM1ixgeIvnfgU8b609BHjefSyZFouRGLmj2V+H6r96iE8nljN4YyUPHh5hzz1T8IbBIEOP\nNEwiSg5KcLOe20w4cH+UXJKcuzaKyel6PPfaC0aOhIMfUn+2nTTrBp9DkslEOXN46rafSy+FXxeU\n87V/aZ2Ld1KSbFlrFwIbWjx9LjDb/fts4LxULEu6KBzm38kQARI05asgdyfuTv3AJ50v0f94NUVJ\nUSwG48Zhg06Ca4NKcLOaG8/GwhQfsASDPDDHcMm2ne9K0OOT8liM+pE7mjendPsJBtkzZPjxNh0I\nibe8rNna11q7vdKzGmh10tcYU2qMqTLGVK1fv97D4ch29R/XcCdlrP5r5+u/2pLV8YzFWFPiwaxf\nOAyhEKbeucDB1GdHgpvVsfSSG8+8hq5dsPIVsRjJMTuSilTPOmdtPMNhXlkSotBt85DS7SdLD4Sy\nNpY9WFoK5N3GX7aNf5tprS221hb37ds3HcPp8WacVsHlBdPZ/6yu1X+1JqvjGQ5T9YEz62dT+SUK\nKbnAId2yOpZe8yKe4TCfbnSSCi9mnbM1no89BhtX1LC4uJNtHnYlSw+EsjWWPZmXrR9qjDFha23c\nGBMG1nm4LOmA99+HQw6BPDX+2MnixdAYd3r3DJ1RCjM72O5hV9pqgCrZyaN4rn2rhheCZUxYWEru\n/Sn8/ctStbUwcSKEh1aw6DUgn9RvP9sT59IUb/MizXj5dfs4cDHwW/fPxzxclnTAZ+/Embt5DFSr\nx1Zzt90GT+5ZwadPA71QUiRptWYNnLCugiuvhPxioLiH//7F46wbOoa82rnMerYf+fkeLUcHQpIG\nqWr98BDwD+AwY8xqY8yPcZKs040xK4DT3MeSYfX1MGFlOd+u1ZU3zX3yCTzyiHNw26tXpkcjPdG9\n9zrdWNT41hH7UTmHrqvk70dGGDIk06MR6ZqUzGxZa8e28U+npuL9JUWCQQoTCSaBU0EXjTo/gRb3\nWOxp4nEaho1hXzuXKVM00yfp17Q6zhm/GcO7J83loIN6+O9gMAiJBAe5D4+uioLRfkqymzrI9yAv\n3x/jkTzvrnbKVvXTyum/ppJZB0U48MBMj0Z6ok9Lyzm6vpLfBDTbTCzGq/3VC1D8RclWD/HnP8Np\nFzlX3gRNii9Zz1ZuX63C+5y+Wqd/qB47kmbu72D/Z5zfwYPn63fwsUVhlqzSfkr8RcmWX8WdW37Y\neDW//jVcfLFzB5Dzhjk3vc2mFgSeicVo8rCvkcjuPPWnGH8t0CzOdhs2OFcfHtqrBluq/ZT4hy7+\n96ty55YfL58a4dplM7joIrjnHigo0JU3XwqH+WhdiIPdvka5OoKWNNmwAX76U3jwwTCP9glRVJuA\nQs3iTJnitHvoU1VB7vai+J6+nxJf0MyW3zS7j5hJJjl5WRSLYfajQQoKMj247sVa+HRxDXP3KsO8\noSNo8Zg72/zcA9UMHuxc/RqJwPnHa7aZeJzPBpXw3Jxqpk1DVx+K72hmy29iMeLjp9LrpXkU2a00\n5BeRP2oE5tZbMz2ybueFF+C0jRXccw/kHImOoMVT9dPKyV9YyQcLI/Q5YgZPPQVHHgmg2ebENeX0\nfq+SO/aJMOLqGZkejkjKaWbLRzZtgkuvCzPvxRAB69xHLL+pZ5+WaFM8ztcvLGFwn2rGj8/0YMTP\nbLMLMXJIMpkoS94xHDms5xbBf8ldN4H7nQsERtVGyS/o2RcIiD8p2cp27qmJlx52Tk3cdx+ceEgN\nyVIP7iPmI7U/L2fg55XcPyBCIJDp0YhfLV8Oo4tjzGEcdUZF8F8Ri/HpibpAQPxPyVaWq7+2nOTC\nSt4bG2GPPeDVV2HwBxXk3TXdKXxI8Y2ms557JL3PI86RdPGbutReUsg9+Nkac2qPjjgC5r8TZsiJ\nIQJqZfAVGwrDPF8VIujFzd9FuhElW9lq+6mJe3ecmlj2vuHY7ylp2KVYjK0jdCQtHikvx75Syd+G\nRLjpJhg71pndGtynBlPWw4vgWzFlCvRK1LBhVBlG60Z8TAXyWWjTJigfGWPonKmcb+YRtFudpGHE\nCFAh/K6Fw7yzKkQxCZKFAXJ0JC2p4N5iBsAAF30R5SKi8EgAZtfpZseteOwxmDMHDrmhghHXu09q\n3YhPaWYrG7inJqiuZv58GDwY/vBQmIOG6NRER23ZAuuW1jB/QBk5avcgKbBtG/zpFzEezt3RINcG\nNWO6K5+/51ygcuqgaq6+OtOjEfGekq1s4DYofemUCGeeyZe1WccdpFMTHXX//fCDhgp6PaCaNumk\nZgc/L77o/BpNuTnMXt/YcYsZU6+Dn11ZfF45xzRU8vDACPn5mR6NiPd0GrE7a3FqwmlQGsWuCmCO\n1amJjmpqgttvh2OPhWHDMj0ayVpuXdbzJ0U4fcUMBgyAJ5+E4ffWwFllUFoKM2c6SZnszN2nneY+\n7PNoFEzUmZ2vq8vo0ES8pJmtbmzz2zHeOHhHMXdToXNqwujURMfF42z4jxK2xKqZOjXTg5Gs1Pzu\nDDbJaSucuzN8FA9y9tk4Bz/TNWO6K58vjvG3QrXBkJ5HyVY3NX8+DDotzFsf7bgsOrdBpyY6rbyc\nfd6r5NY9I5x3XqYHI9norb/FeKb3joOfZEAHPx3109+EWb8tRADVmkrPomSru3DrQDavqKa0lC9r\ns0aV1JAzWZdFd1qz2YgckkzYHCU3T321ZDea1WV9/jlMmgRH/WeYdfUhioxz8JOzTYlCR2y/+vCk\nw3QvSOl5VLPVXbh1IPO+E+HerTO48kq48UYIBFSX1SWxGEydSmLuPAJNW7HBIsz5apEhu+FelLJ8\nXISTls6gthZ+9jMY92EN5kDVZXVIPE7DBWOY9sFchg7txyGLKmB7Ubz2adJDKNnKtLb689wRgN+p\nYLTLwmE22hBfa0rQkBcgX1eJya602B4PfzHKOqIkCwLk3F6HbhrdCeXl5L5WyU9MhGMXzNDVh9Ij\neX4a0RizyhjzjjHmX8aYKq+Xl21evj/GvKIddSDqz5N6K1+v4e6cMv79jE5byK5teSfGP7+9Y3ts\nLCjCjhtPzsfaHjusxSn8Mhtl6JE6hS89U7pqtr5nrR1qrS1O0/K6vU2bYOJEOHlsmLp8pw5E/XlS\nr7YWhlVX8OYPp9P3NF0lJq2z1vm1OPx7Yd5Y5l6UUhggrzGB6aXtsVNiMepHqtGrCKhAPr3cotuX\n51ZzxBFwzz1wxRVOEbwKRj0Qj/PFUSWE6qq5/PJMD0a6HXd7XPV6NWefDSNHwt57w6iT3ItSdIeB\nrgmHeWVJiEISJAt0ICk9Wzpqtiww3xhjgbustTOb/6MxphQoBTjwwAPTMJzMqb+2nPyFlby7MELw\nsBm8+qrTYNNPdSDdKZ6N15dzwMeV3PONCIMGzcjoWLJRd4qlFxqvLyfnlUqePSHCK8EZ3H47XHYZ\n5OX5Z3tsLt3xfOwxaFxRw+LiMo65RxcUpJLft00/MtZabxdgzP7W2jXGmK8DC4CfWmsXtvba4uJi\nW1Xlw7KuZkW3O+lmXZONMYtTeao3Y/HMkvXttVTG01fbZhb+fmTbtrlhAwwc6ExiLVoEBQWeLSor\nadv0j/bG0vPTiNbaNe6f64C/A8d4vczuZNMmuOKCGHNQ1+S0icWwY3esb6v1La41a2Di6doePeOe\nmr320mpqa2HWLCVaIuBxsmWM2cMYs+f2vwNnAEu9XGbGNWuGuGABHHEE/OHBMAOGhAgYdU1Oi3CY\nT/4dosAmaMwPYLS+ey53e2xcXc3tt8Phh8OfF4Q57Ghtj55w+wUOqogwbRoMHZrpAYl0D17PbO0L\nVBpj3gYWAU9Za//P42VmltsM8aVTIpxxhnPGorIShh1UgylTEXy6fPxmDQ/sUYZ9Teu7R3O//P82\nJMLll8NJJ8G770LxAdoeU6rFfSMnE+X6G9TmQWQ7TwvkrbUxYIiXy+g2WjRDPHlZFEsU+3EAc1zd\nzu0GfFR02x0tXgwln1Vwyy2QXwwUa333OC22x9Eboowmin0hgDlI22PKuXdqqJ87j8KmrSQDReSM\n1J0aRLZT64cU2fx2jDcO3tEMsalQN6nNlNtugz33hEsvzfRIJBOSSXggEuPRgq82C9b26JFwmJW1\nIfLcOzXovpEiO1OylQILFsDg08O89ZHbDDEQILdBO5tMWP1mnLKHSrh8XDW9emV6NJIWzeok33oL\nhg2Di64Mk7+PmgWnS20tvPdSDX/dpwz+oVOzIi0p2eoMd+e+eUU1EyfyZW3WqBK3GaLqQDLm40vK\nOZ5KfrklkumhSLq4dZKVZ0QoLnYuKvzzn+HcY9UsOF2mTIHzmio47Lnp5BfrTg0iLelG1J3hFt3O\n+06Ee7bO4Ior4MYbIRhUHUjGuDU6x7sP93wgCg9Eu3XvJOmiFnVZJ7wTpYkodmMAc1EdXKTt0XPx\nOJ+dNobn35vLtBv66epDkTZoZqsjWlxxc9EXUZqSht//KaiLbjItFuO9I3fU6Kh3kv99OD/G8/u2\nUie5SjFPl8Q15fR+r5I79olw1VWZHo1I96VkqwMWzooxr+irRbf6Qs+8hj5hFn/g1Mypd5IPNavL\n2roVpk2DgaeGWfW5U5elOsk0cw88A/dHySXJqNooBYVq9SDSFiVbu9KiNqtkTJi6fBXddkePPAJF\nW2r45CzV6PhSeTlUVrLqRxEGDYKbboKxY2H8qU5dluok0ywW49MTNZMs0l6q2dqVVmqzRi2vwRxQ\nBqW6sWp3Ya3Tzqf+2xUsfQLnEEI1Ov7Q4j6G/Z+JspIoTQUBcmfX4aebuGeT2oIwz1eF+H/u1de6\nS4PIrinZak2LwtuLvohyEVH4U4tia+3cMy8e59/DxxBfMpdf392PHM3V+sq292OsOHcqA96eRxFb\nacgvIveCEeT+Qc0yM2nKFLggUcOGUWX0uVoHniK7o6+mVqg2K4uUl9NrSSU3ByJMmJDpwUgqvfgi\nDBkeZuHbIQIkSBYGyG9KkLOXZlAyad48ePBBWHJ9BX3mTochavUgsjtKtprZvNkp+1BtVhZodmVo\nDkn+KxElEFSBblZzayTXLalmwgQ45RSor4fzjnP61+W8obqsjIrHaRhWwrWXVjN0KFx9daYHJJI9\nlGy5O/iFj1QzeDDcfTdMneo0KFVDxG4sFoNx40jkavbRL5I3OjWSjxdHePRRuPZa56bR4dcqnJkT\nzaBkVnk5uf+o5Ce1EWbNgvz8TA9IJHv0+JqtbdeWk7ewkqULIwQPm0FlJRx3HKjwtpsLh/lkY4j9\n3Xux5Wv2MXu5NZLbj/wuaYhyCVG4JQARNaTNuGY1rDlAmY3CUDUMFumInjuz5Z6GKrjXOQ01mSjv\nLzccd4pOQ2WDbdvg/ZdreChUhn1Ns4/Z6vPP4b8vjDGHcWzdXiOpNgLdSyxG/chm8dEsskiH9chk\na9MmuOICZwdfZ9QnJhv97//CmV9UsNec6RQcrdNLWcM9bW/j1cyeDYcdBrfOCdP/iBDBHLdGUm0E\nupdwmFfeDlFIgmSBalhFOqNnJFvNuk8/9xwccQT84cEwA4aECBh1HM8q8Tjbjishen01w4fD2Wdn\nekDSIdt71x0V4Yc/hG99CxYvhuO/VYMpU41kdzRvHmz8sIZ/Hl1GziLFR6QzekbNVnk5trKSl0+J\ncPqyGRx6KE5t1i01cLwalGaV8nLyXq/klybC9/5nBsZkekDSLi16142IR7FEsW8FMEPrdp6VVI1k\nt1Fb6+RW4aEVLHoVyEfxEekEfydbLXbwJy9zd/CfBDDHaQefVVor0j1cRbrZwFp4+k8xtk2Zyhl1\n89iDrdhgEeb8EZhb1Zy024rHWTdkDHkb5jLr2X66+lCkCzw/jWiMGW6MWW6M+dAY8ytPFtLsNOF2\nK1bAb0tj/L1Zc9KmQqcuy6guq3trEc/16+HOK2M8sacazWaFZvFb+Vqcd/Yp4ceXGhqK1Lsum8R+\nVM6h6yv5+5ERhgxxn2xlXysiu+dpsmWMyQWmA98HBgJjjTEDU74g9ya19ddEuPtuOOEEOPRQuOZ/\nw+Tv4+zgbSBAboN28FnBPe376aURRo+G/feHSZHwl1/WVl/W3ZsbvzfPiTD/xHIGfV7Js8dHuOBE\n9a7LCu6V2gf9X5RckhxdFXUaCAeDX+5riUQyPUqRrGKstd69uTHHATdYa890H18FYK29ubXXFxcX\n26qqqvYvoMVNardLmAB/vLmOCRNg/5+eD+HwznVZumqtVcaYxdba4lS9X6ri2ZAbYPm/6hh8nWLZ\nEamMZ7ti2Ub8dqLTvp2S1m0zHufVYVMZuso55UtRkRPXZPKrr1U8OyXt26Z4pr2x9Po04v7Ap80e\nr3af+5IxptQYU2WMqVq/fn3H3t3tIt6Q75xaqs8r4rMzx1O4ZiX//d/OjAgV6j6dTl2NZ+OoHf18\nGguKaBwznvzVKxk8GMUyzTocy1iM5UftONWbzMmlyeQCsIUiXjlwPAtnr9z5O1unpdKmvfF8bFGY\nJatCBJtfqT1hAhvP3rFt1pkiln1nPLVVzU7jK5Zp06X9rGRExls/WGtnWmuLrbXFffv27dh/Doch\nFCKvyTm1VJhM0OegECasU0uZ0tV45u29o99SXmOCvN46VZgpHYllYyP89DdhXlgcIoizPeYkm8i1\nTSQLAgRJ8EFNiJLR/fjWt+Cmm2DtWnRaKo3aE88NG5wzvIf2qsGWNjvlu3kzvb7hbJuN+QEKbYIX\n/xliv+/0Y8wYeP55sBHFMl26tJ+VjPA62VoDfKPZ4wPc51KnxunPY1QH4g816reUbTZtgh/8wGk0\ne/y3amD79jhgAAwYQM6i18mZXMYPh1fz4IPQvz9cPi3Ifvs7NxInmXT+NLqReKb9/Ofw2Wewz0sV\n5N7ZYhbZ3Tbz3nTiOf7UasrKYNbcIKeeZjB3KpYibbLWevaD01oiBgwACoC3gUFtvf6oo46ykjlA\nlU1h/BXPzEplPFuN5dq1tu6Yk+zJh8dtbq61d93V/rF9VLnW/nPgOLuFImvB2qIia8ePtzYe7+zH\n9TXPt821a+36QSfZfYnb667r2NjqYmttbNg4u9U4sWwKKJa74/m2KWnT3lh6OrNlrW0ELgOeBZYB\nj1hr3/VymSKSHjWXlZO/qJLxsQjPPONct9BeBx0f5siTQgRIUEcAW6erSzMpMa2c3u9W8se9I1xz\nTcf+b2BAmAH/sSOWuhuHyFd53tTUWvs08LTXyxGRNHGvOtzXfXjJtiic0YkGszU11F1cxskPlnLL\nITM5uVp3cEg7N5YB9+HoDVEo7FwszaQyfrumlPCTM/nhx/Ev31NE/N5BXkRSLxZj88Sp5DzRrDXA\niBHQ0W7wFRXsARxdBGfeO53Y/BaXKov3YjGYOpXE3HkEmroWS4Bxy+Hbj0/nkyHwGw+GK5KtMn41\noohkmXCYzxqc00bJgq6fNrriCmhqgpk3qnVA2rlXdBc0JdiW0/VYHnYYXHihU1O/8X3FU2Q7JVsi\n0mHbPq3hTsrY+kLXrxodMMC589L+9zmd59U6IL2a1tYQpYx7L03NFcBXXeVcobriYrWCENlOpxFF\npMNu+k4FL26EnxwPHN/Fm7gHg8xu3nk+GnV+1J08Ld65sYLLHoe5pwCjuhhLYOhxQSwJWOQ+oXiK\naGZLRDruvfdgYKrucureCSKR43Qnp0g3GU+nZcucP1MZz4+P33EnAcVTRMmWiHRQck2c298q4bvf\nTFEtTjhMQzBEfjJBQ65aB6TbJ2/EeZkSDtkzdfFcvcmp6bMBxVMElGyJSAd98atyhiUrGfNB6mpx\n1i11asDeiurOAek25PFyTqCSwt+lJp7WwuYPa3jmm7qzh8h2qtkSkfZxezKF3IcDX46CSU0tztWH\nVfDEB7Duv4C8rtcNSTu48Ry+/XGKaquWLIHv11Vwz7XAEJxLE0V6OM1siUj7uLVVNujU4myliK3n\nd70Wp6GPd4ftAAAH50lEQVQBnngCzjkH8nT4lz5uPBsLnHg25KemtmrePOfWiOeck4pBiviDki0R\naR+3J5OpT5AsDFBIgpcWh7D7dq0W5415cf7+eQmjS3SqKa3ceOY1Oj22choS1DZ0sbYqHufsW0o4\n5+hqvv711A1VJNsp2RKR9qupgbIyct54nXeGlVH3cTVz5nTtLW3EqRk6/R/qx5R2bjy/eO51ZgfK\neHt+NY2NnX+7TVeUc+SWSm7MUyxFmtOkvYi0n3tbFoAjFk7nJyfBsilw+uA4+/5sDMyd2/6ZEbdm\n6ET3Ye49UbhH/ZjSyo3n3kDoL9M59UL447VxprzWuVhur+cb+lrq6vlE/EAzWyLSKbm5cN99znfp\n4vM61v1982b4bWmMh3LGsVX9mLqFCy5wbrVT8Pty7Csd6/y+cFaMp3qpt5ZIW5RsiUinHTY0SF3C\ncNbHUUwy6VzNZowz09Fc3LlPXtOaau67Dw49FK66I8zXDw4RNAlnBkT9mDIrGOSRRw1lySjG7j6W\nVFfzwQfwgx9AyZgwnydDFBn11hJpjZItEem8WIzk2B3d37dQxMN54xl9zErKy+Gll9yzSOXObMnf\nhkT48Y+d+yG+8QacOrgGM6kM1I8p87ZfnVi4I5YPMJ6SA1dSVgZz5sAnn+DEsrKSyjMiDBrkxPh3\nv4OxpzixVG8tka9SzZaIdF44TE4vt1t4YYCibQn2OyzEss/78ej1sMUGCeLc99AAo2qjjCKKfSuA\nOaZupxow9WPKsO1XJzY4s1NF9Qm+fXSIPfbpx0MPwe137RzLE96J0kAUuy2AubIOUCxF2qKZLRHp\nGveKNvPG65hJZZx0aDVLlkBtLVTOjvHWt3fMfNmgU8tjVMvTPW2P5etOLI/av5qnn4YNG+Cj+TGW\nHzWO+lwnlsmAG8tViqXI7mhmS0S6po3Zqd694fT/F4Z/hGC5U5dl6lXL0621EcvcXBh8ehgqQvCW\nE8ucbYqlSHtpZktEvOXOlqguywcUS5FO8WxmyxhzA3ApsN596mpr7dNeLU9EuinVZfmHYinSKV6f\nRrzdWnurx8sQERER6bZ0GlFERETEQ14nW5cZY5YYY+4zxvRu7QXGmFJjTJUxpmr9+vWtvUSyiOLp\nH4qlvyie/qFYZp8uJVvGmOeMMUtb+TkXiAIHA0OBOHBba+9hrZ1prS221hb37du3K8ORbkDx9A/F\n0l8UT/9QLLNPl2q2rLWnted1xpi7gSe7siwRERGRbOTZaURjTLjZwxHAUq+WJSIiItJdeXk14u+N\nMUMBC6wCJnq4LBEREZFuybNky1p7kVfvLSIiIpIt1PpBRERExENKtkREREQ8pGRLRERExENKtkRE\nREQ8pGRLRERExENKtkREREQ8pGRLRERExENKtkREREQ8pGRLRERExENKtkREREQ8pGRLRERExENK\ntkREREQ8pGRLRERExENKtkREREQ8pGRLRERExENKtkREREQ8pGRLRERExENKtkREREQ81KVkyxhz\noTHmXWNM0hhT3OLfrjLGfGiMWW6MObNrwxQRERHJTnld/P9LgfOBu5o/aYwZCIwBBgH7Ac8ZYw61\n1jZ1cXkiIiIiWaVLM1vW2mXW2uWt/NO5wMPW2npr7UrgQ+CYrixLREREJBt5VbO1P/Bps8er3ee+\nwhhTaoypMsZUrV+/3qPhSLoonv6hWPqL4ukfimX22W2yZYx5zhiztJWfc1MxAGvtTGttsbW2uG/f\nvql4S8kgxdM/FEt/UTz9Q7HMPrut2bLWntaJ910DfKPZ4wPc50RERER6FK9OIz4OjDHGFBpjBgCH\nAIs8WpaIiIhIt9XV1g8jjDGrgeOAp4wxzwJYa98FHgHeA/4P+ImuRBQREZGeyFhrMz2GLxlj1gMf\nd/K/9wE+S+FwupN0fbZvWmtTVgCgeLYp6+KpWLYp62IJimcb0vm5tG16r1ttm90q2eoKY0yVtbZ4\n96/MPn7+bG3x82f282drjZ8/r58/W1v8+pn9+rl2xc+fubt9Nt2uR0RERMRDSrZEREREPOSnZGtm\npgfgIT9/trb4+TP7+bO1xs+f18+frS1+/cx+/Vy74ufP3K0+m29qtkRERES6Iz/NbImIiIh0O0q2\nRERERDzki2TLGDPcGLPcGPOhMeZXmR5PVxhj7jPGrDPGLG323N7GmAXGmBXun70zOUYvKZb+onj6\nh2LpL4pnemV9smWMyQWmA98HBgJjjTEDMzuqLpkFDG/x3K+A5621hwDPu499R7H0F8XTPxRLf1E8\n0y/rky3gGOBDa23MWrsNeBg4N8Nj6jRr7UJgQ4unzwVmu3+fDZyX1kGlj2LpL4qnfyiW/qJ4ppkf\nkq39gU+bPV7tPucn+1pr4+7fq4F9MzkYDymW/qJ4+odi6S+KZ5r5IdnqUazTq0P9OnxAsfQXxdM/\nFEt/6Q7x9EOytQb4RrPHB7jP+UmNMSYM4P65LsPj8Ypi6S+Kp38olv6ieKaZH5KtN4FDjDEDjDEF\nwBjg8QyPKdUeBy52/34x8FgGx+IlxdJfFE//UCz9RfFMN2tt1v8AZwEfAB8B12R6PF38LA8BcaAB\n5zz6j4F9cK6mWAE8B+yd6XEqloql4tmz4qlY+utH8Uzvj27XIyIiIuIhP5xGFBEREem2lGyJiIiI\neEjJloiIiIiHlGyJiIiIeEjJloiIiIiHlGyJiIiIeEjJloiIiIiH/j9tOtAaaDcl8AAAAABJRU5E\nrkJggg==\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7ffa53995890>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Generating any path from a set of options. The options, documented below are referred to as \"words\" and consist of\n",
    "# different turning options (3 segments are connected, segments are either left turn, straight, or right turn).\n",
    "\n",
    "# Words: LSL = 0, LSR = 1, RSL = 2, RSR = 3, RLR = 4, LRL = 5\n",
    "words = [0,1,2,3,4,5]\n",
    "\n",
    "#starting location (x,y,yaw)\n",
    "q0 = (0, 0, 0)\n",
    "\n",
    "#goal location (x,y,yaw)\n",
    "q1 = (10, 10, -1.57)\n",
    "\n",
    "#radius of feasible turn\n",
    "turning_radius = 2.0\n",
    "\n",
    "#sampling \"rate\" or distance\n",
    "step_size = 1.5\n",
    "\n",
    "fig,ax = plt.subplots(1,5,sharex=True,sharey=True)\n",
    "fig.set_size_inches(10,3)\n",
    "\n",
    "#generate a figure for each of the options\n",
    "for a,word in zip(ax,words):\n",
    "    path = dubins.path(q0, q1, turning_radius, word)\n",
    "    # Note! If a path is not feasible, path will be a Nonetype object, which will need to be handled\n",
    "    if path != None:\n",
    "        configurations, _ = path.sample_many(step_size)\n",
    "        configurations = np.array(configurations)\n",
    "        a.axis('equal')\n",
    "        a.plot(configurations[:,0], configurations[:,1], 'b-')\n",
    "        a.plot(configurations[:,0], configurations[:,1], 'r*')\n",
    "        \n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Making our Own Path Set Class\n",
    "\n",
    "The interface for the dubins curve library is nice, but not quite ready in this form factor to interface with our system. We will now create three different path generation classes: Path_Generator, Dubins_Path_Generator, and Dubins_EqualPath_Generator.\n",
    "\n",
    "**Path_Generator** creates the naive point to point connections from one point to another.\n",
    "\n",
    "**Dubins_Path_Generator** create point-to-point connections, but calls upon the dubins library to generate paths.\n",
    "\n",
    "**Dubins_EqualPath_Generator** controls for the number of sample points along the dubins trajectories so that all trajectories have the same number of samples available. This is useful if we would like to control for the number of points along a curve to consider in information space.\n",
    "\n",
    "All classes can be parameterized by selecting the number of paths to generate (frontier_size), the distance between the center point and the horizon (horizon_length), vehicle dynamics (turning_radius), and sampling rate (sample_step). The pose of the robot is provided at every timestep in order to generate the world-coordinate sensitive list of path sets."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 101,
   "metadata": {},
   "outputs": [],
   "source": [
    "'''The Path_Generator class which creates naive point-to-point straightline paths'''\n",
    "class Path_Generator:    \n",
    "    def __init__(self, frontier_size, horizon_length, turning_radius, sample_step, extent):\n",
    "        ''' Initialize a path generator\n",
    "        Input:\n",
    "            frontier_size (int) the number of points on the frontier we should consider for navigation\n",
    "            horizon_length (float) distance between the vehicle and the horizon to consider\n",
    "            turning_radius (float) the feasible turning radius for the vehicle\n",
    "            sample_step (float) the unit length along the path from which to draw a sample\n",
    "            extent (list of floats) the world boundaries\n",
    "        '''\n",
    "\n",
    "        # the parameters for the dubin trajectory\n",
    "        self.fs = frontier_size\n",
    "        self.hl = horizon_length\n",
    "        self.tr = turning_radius\n",
    "        self.ss = sample_step\n",
    "        self.extent = extent\n",
    "\n",
    "        # Global variables\n",
    "        self.goals = [] #The frontier coordinates\n",
    "        self.samples = {} #The sample points which form the paths\n",
    "        self.cp = (0,0,0) #The current pose of the vehicle\n",
    "\n",
    "    def generate_frontier_points(self):\n",
    "        '''From the frontier_size and horizon_length, generate the frontier points to goal'''\n",
    "        angle = np.linspace(-2.35,2.35,self.fs) #fix the possibilities to 75% of the unit circle, ignoring points directly behind the vehicle\n",
    "        goals = []\n",
    "        for a in angle:\n",
    "            x = self.hl*np.cos(self.cp[2]+a)+self.cp[0]\n",
    "            if x >= self.extent[1]-3*self.tr:\n",
    "                x = self.extent[1]-3*self.tr\n",
    "                y = (x-self.cp[0])*np.sin(self.cp[2]+a)+self.cp[1]\n",
    "            elif x <= self.extent[0]+3*self.tr:\n",
    "                x = self.extent[0]+3*self.tr\n",
    "                y = (x-self.cp[0])*np.sin(self.cp[2]+a)+self.cp[1]\n",
    "            else:\n",
    "                y = self.hl*np.sin(self.cp[2]+a)+self.cp[1]\n",
    "                if y >= self.extent[3]-3*self.tr:\n",
    "                    y = self.extent[3]-3*self.tr\n",
    "                    x = (y-self.cp[1])*-np.cos(self.cp[2]+a)+self.cp[0]\n",
    "                elif y <= self.extent[2]+3*self.tr:\n",
    "                    y = self.extent[2]+3*self.tr\n",
    "                    x = (y-self.cp[1])*-np.cos(self.cp[2]+a)+self.cp[0]\n",
    "            p = self.cp[2]+a\n",
    "            if np.linalg.norm([self.cp[0]-x, self.cp[1]-y]) <= 3*self.tr:\n",
    "                pass\n",
    "            elif x > self.extent[1]-3*self.tr or x < self.extent[0]+3*self.tr:\n",
    "                pass\n",
    "            elif y > self.extent[3]-3*self.tr or y < self.extent[2]+3*self.tr:\n",
    "                pass\n",
    "            else:\n",
    "#                 print (x,y,p)\n",
    "                goals.append((x,y,p))\n",
    "        \n",
    "        self.goals = goals\n",
    "        return self.goals\n",
    "\n",
    "    def make_sample_paths(self):\n",
    "        '''Connect the current_pose to the goal places'''\n",
    "        cp = np.array(self.cp)\n",
    "        coords = {}\n",
    "        for i,goal in enumerate(self.goals):\n",
    "            g = np.array(goal)\n",
    "            distance = np.sqrt((cp[0]-g[0])**2 + (cp[1]-g[1])**2)\n",
    "            samples = int(round(distance/self.ss))\n",
    "\n",
    "            # Don't include the start location but do include the end point\n",
    "            for j in range(0,samples):\n",
    "                x = cp[0]+((j+1)*self.ss)*np.cos(g[2])\n",
    "                y = cp[1]+((j+1)*self.ss)*np.sin(g[2])\n",
    "                a = g[2]\n",
    "                try: \n",
    "                    coords[i].append((x,y,a))\n",
    "                except:\n",
    "                    coords[i] = []\n",
    "                    coords[i].append((x,y,a))\n",
    "        self.samples = coords\n",
    "        return self.samples\n",
    "\n",
    "    def get_path_set(self, current_pose):\n",
    "        '''Primary interface for getting list of path sample points for evaluation\n",
    "        Input:\n",
    "            current_pose (tuple of x, y, z, a which are floats) current location of the robot in world coordinates\n",
    "        Output:\n",
    "            paths (dictionary of frontier keys and sample points)\n",
    "        '''\n",
    "        self.cp = current_pose\n",
    "        self.generate_frontier_points()\n",
    "        paths = self.make_sample_paths()\n",
    "        return paths\n",
    "\n",
    "    def get_frontier_points(self):\n",
    "        ''' Method to access the goal points'''\n",
    "        return self.goals\n",
    "\n",
    "    def get_sample_points(self):\n",
    "        return self.samples"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(0.15000000000000002, 0.3137159878184177, 9.647368421052635)\n",
      "(0.15000000000000002, 0.4858387428429182, 9.894736842105267)\n",
      "(0.15000000000000002, 0.6375157218843859, 10.142105263157898)\n",
      "(0.15000000000000002, 0.7595128696071771, 10.389473684210529)\n",
      "(2.3897807642351188, 0.22409811969496765, 12.615789473684213)\n",
      "(2.3260335303972193, 0.5886741878131839, 12.863157894736846)\n",
      "(2.1749590876242273, 0.9265438846007921, 13.110526315789476)\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7ffa52f7ea90>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "class Dubins_Path_Generator(Path_Generator):\n",
    "    '''\n",
    "    The Dubins_Path_Generator class, which inherits from the Path_Generator class. Replaces the make_sample_paths\n",
    "    method with paths generated using the dubins library\n",
    "    '''\n",
    "    \n",
    "    def buffered_paths(self):\n",
    "        coords = {}\n",
    "        for i,goal in enumerate(self.goals):            \n",
    "            path = dubins.shortest_path(self.cp, goal, self.tr)\n",
    "            configurations, _ = path.sample_many(self.ss)\n",
    "            configurations.append(goal)\n",
    "            temp = []\n",
    "            for config in configurations:\n",
    "                if config[0] > self.extent[0] and config[0] < self.extent[1] and config[1] > self.extent[2] and config[1] < self.extent[3]:\n",
    "#                     print config\n",
    "                    temp.append(config)\n",
    "                else:\n",
    "                    print 'here'\n",
    "                    temp = []\n",
    "                    break\n",
    "\n",
    "            if len(temp) < 2:\n",
    "                pass\n",
    "            else:\n",
    "                print temp[-1]\n",
    "                coords[i] = temp\n",
    "        return coords    \n",
    "        \n",
    "    def make_sample_paths(self):\n",
    "        '''Connect the current_pose to the goal places'''\n",
    "        coords = self.buffered_paths()\n",
    "        \n",
    "        if len(coords) == 0:\n",
    "            print 'no viable path'\n",
    "            \n",
    "        self.samples = coords\n",
    "        return coords\n",
    "\n",
    "    \n",
    "DPG = Dubins_Path_Generator(frontier_size=20, horizon_length=1.5, turning_radius=0.05, sample_step=0.5, extent=[0,10,0,10])\n",
    "# s = DPG.get_path_set((9.8,9.8,np.pi/2))\n",
    "s = DPG.get_path_set((0.8916120592296812, 0.15000000000000002, 10.760526315789477))\n",
    "\n",
    "\n",
    "m = DPG.get_frontier_points()\n",
    "m = np.array(m)\n",
    "l = DPG.get_sample_points()\n",
    "\n",
    "fig,ax = plt.subplots(1,1)\n",
    "ax.axis('equal')\n",
    "for key,val in l.items():\n",
    "    f = np.array(val)\n",
    "    plt.plot(f[:,0], f[:,1], 'b*')\n",
    "    \n",
    "plt.plot(m[:,0], m[:,1], 'r*')\n",
    "plt.plot(DPG.cp[0], DPG.cp[1], 'g*')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "__init__() takes exactly 6 arguments (5 given)",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-6-1b5548fe0dea>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m     29\u001b[0m         \u001b[0;32mreturn\u001b[0m \u001b[0mcoords\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     30\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 31\u001b[0;31m \u001b[0mDEPG\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mDubins_EqualPath_Generator\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfrontier_size\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m6\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mhorizon_length\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m5\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mturning_radius\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msample_step\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m0.5\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     32\u001b[0m \u001b[0ms\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mDEPG\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mget_path_set\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m10\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m10\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     33\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mTypeError\u001b[0m: __init__() takes exactly 6 arguments (5 given)"
     ]
    }
   ],
   "source": [
    "class Dubins_EqualPath_Generator(Path_Generator):\n",
    "    '''\n",
    "    The Dubins_EqualPath_Generator class which inherits from Path_Generator. Modifies Dubin Curve paths so that all\n",
    "    options have an equal number of sampling points\n",
    "    '''\n",
    "        \n",
    "    def make_sample_paths(self):\n",
    "        '''Connect the current_pose to the goal places'''\n",
    "        coords = {}\n",
    "        for i,goal in enumerate(self.goals):\n",
    "            g = (goal[0],goal[1],self.cp[2])\n",
    "            path = dubins.shortest_path(self.cp, goal, self.tr)\n",
    "            configurations, _ = path.sample_many(self.ss)\n",
    "            coords[i] = configurations\n",
    "        \n",
    "        # find the \"shortest\" path in sample space\n",
    "        current_min = 1000\n",
    "        for key,path in coords.items():\n",
    "            if len(path) < current_min:\n",
    "                current_min = len(path)\n",
    "        \n",
    "        # limit all paths to the shortest path in sample space\n",
    "        for key,path in coords.items():\n",
    "            if len(path) > current_min:\n",
    "                path = path[0:current_min]\n",
    "                coords[key]=path\n",
    "        \n",
    "        self.samples = coords\n",
    "        return coords\n",
    "\n",
    "DEPG = Dubins_EqualPath_Generator(frontier_size=6, horizon_length=5, turning_radius=1, sample_step=0.5)\n",
    "s = DEPG.get_path_set((-10,-10,0))\n",
    "\n",
    "m = DEPG.get_frontier_points()\n",
    "m = np.array(m)\n",
    "l = DEPG.get_sample_points()\n",
    "\n",
    "fig,ax = plt.subplots(1,1)\n",
    "ax.axis('equal')\n",
    "for key,val in l.items():\n",
    "    f = np.array(val)\n",
    "    plt.plot(f[:,0], f[:,1], 'b*')\n",
    "    \n",
    "plt.plot(m[:,0], m[:,1], 'r*')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
